#define UP 1
#define RIGHT 2
#define LEFT 4
#define DOWN 8
#define iUP 0
#define iRIGHT 1
#define iLEFT 2
#define iDOWN 3

#include <iostream>
using namespace std;

//bottom: row + 1, col * 2 + 1
//top: row ==, col * 2 + 1
//left: row + 1, col * 2
//right: row + 1, col * 2 + 2

int sonar(int x, int y, int dir, char* maze, int sizex, int sizey, int offsetx, int offsety) {
	y = (-y - offsety + 2*sizey)%sizey +1;
	x = ((x + offsetx)%sizex)*2;
	sizex = 2*sizex + 1;
	switch(dir) {
	case iUP:
		return maze[(y-1)*sizex+(x+1)];
	case iLEFT:
		return maze[(y)*sizex+(x)];
	case iDOWN:
		return maze[(y)*sizex+(x+1)];
	case iRIGHT:
		return maze[(y)*sizex+(x+2)];
	default:
		return 1;
	}
}

void putwall(int x, int y, int dir, char* maze, int sizex, int sizey, int offsetx, int offsety) {
	y = (-y - offsety + 2*sizey)%sizey +1;
	x = ((x + offsetx)%sizex)*2;
	sizex = 2*sizex + 1;
	switch(dir) {
	case iUP:
		maze[(y-1)*sizex+(x+1)]=1;
		break;
	case iLEFT:
		maze[(y)*sizex+(x)]=1;
		break;
	case iDOWN:
		maze[(y)*sizex+(x+1)]=1;
		break;
	case iRIGHT:
		maze[(y)*sizex+(x+2)]=1;
	}
	return;
}

void mark(int x, int y, int dir, char* maze, int sizex, int sizey, int offsetx, int offsety) {
	y = (-y - offsety + 2*sizey)%sizey +1;
	x = ((x + offsetx)%sizex)*2;
	sizex = 2*sizex + 1;
	switch(dir) {
	case iUP:
		maze[(y-1)*sizex+(x+1)]=2;
		break;
	case iLEFT:
		maze[(y)*sizex+(x)]=2;
		break;
	case iDOWN:
		maze[(y)*sizex+(x+1)]=2;
		break;
	case iRIGHT:
		maze[(y)*sizex+(x+2)]=2;
	}
	return;
}

void drawmap(char* maze, int sizex, int sizey, int posx, int posy) {
	system("cls");
	for(int y = 0; y<(sizey+1); y++) {
		for(int x = 0; x<(2*sizex+1); x++) {
			if((x==2*posx+1) && (y==sizey-posy+1)) {cout << "*"; continue;}
			if(maze[y*(2*sizex+1)+x]==1) {
				if(x%2) cout << "_";
				else cout << "|";
			} else cout << " ";
		}
		cout << "\n";
	}
	cout << "\n\n";
}

int xdelta(int dir) {
	switch(dir) {
	case iRIGHT:
		return 1;
	case iLEFT:
		return -1;
	default:
		return 0;
	}
}

int ydelta(int dir) {
	switch(dir) {
	case iDOWN:
		return -1;
	case iUP:
		return 1;
	default:
		return 0;
	}
}

int left(int dir) {
	switch(dir) {
	case iUP:
		return iLEFT;
	case iRIGHT:
		return iUP;
	case iDOWN:
		return iRIGHT;
	case iLEFT:
		return iDOWN;
	default:
		return iUP;
	}
}

int right(int dir) {
	switch(dir) {
	case iUP:
		return iRIGHT;
	case iRIGHT:
		return iDOWN;
	case iDOWN:
		return iLEFT;
	case iLEFT:
		return iUP;
	default:
		return iUP;
	}
}

int back(int dir) {
	switch(dir) {
	case iUP:
		return iDOWN;
	case iRIGHT:
		return iLEFT;
	case iDOWN:
		return iUP;
	case iLEFT:
		return iRIGHT;
	default:
		return iUP;
	}
}

int main(int argc, char** args) {
	//the maze
	int sizex = 20;
	int sizey = 20;
	char maze[21][41] = { //index with maze[-y][x]
{0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0}, 
{1,1,0,1,0,1,0,0,0,0,1,0,1,0,0,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,0,1,0,1,0,0,1,0,0,0,1},
{1,0,0,1,0,1,0,1,1,1,0,1,0,0,1,1,0,1,0,1,0,1,0,0,0,1,1,0,1,1,0,0,1,0,0,0,1,0,1,0,1},
{1,0,1,0,1,1,0,0,0,1,0,1,0,0,0,1,0,0,0,1,0,0,1,1,0,1,0,1,0,1,0,1,1,0,1,1,1,0,1,0,1},
{1,0,1,1,0,1,0,1,0,1,0,0,1,0,0,1,1,0,1,0,1,1,0,1,0,1,0,1,0,1,0,0,1,1,0,1,0,1,1,0,1},
{1,0,1,0,0,1,0,1,0,1,0,1,1,1,0,1,1,0,1,0,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1,1},
{1,0,1,0,0,0,1,0,0,1,0,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,0,1,0,1,0,1,0,0,1,1,0,1,0,0,1},
{1,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,0,1,0,1,0,0,1,0,0,1,0,1,0,1,1},
{1,0,1,0,1,1,1,0,1,0,1,1,0,1,0,1,0,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,1,0,1,0,0,0,0,0,1},
{1,0,1,0,0,0,1,0,1,1,0,1,0,1,0,1,0,1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1},
{1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,1,0,1,0,1,1,0,1,0,0,1,0,1,0,1,0,1},
{1,1,0,1,1,1,0,1,1,0,0,1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,0,1,1,1,0,0,1,0,1,0,1,0,1,0,1},
{1,0,1,0,0,1,0,0,1,0,0,1,0,1,0,1,0,1,0,0,0,1,1,0,1,1,0,1,0,0,0,1,1,0,1,0,1,0,1,0,1},
{1,1,0,1,1,0,0,1,1,0,0,1,0,1,0,1,0,1,0,1,1,1,0,0,0,1,0,0,1,0,1,0,1,0,1,1,0,1,1,0,1},
{1,0,0,1,0,0,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,1,0,1,1,0,1,0,0,0,1,1},
{1,0,0,0,1,0,1,1,0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,1,0,1,0,1,1,0,1,0,0,1,0,1,0,1,1,0,1},
{1,1,1,0,1,1,0,1,0,0,1,0,1,0,1,0,0,1,0,0,1,1,0,1,0,0,0,1,1,0,1,0,1,1,0,1,0,1,0,0,1},
{1,0,0,1,1,0,0,0,1,0,1,0,1,0,1,1,0,1,1,0,1,1,0,1,0,1,0,0,1,0,1,0,0,1,0,0,1,0,1,0,1},
{1,1,0,0,1,0,1,1,1,1,0,0,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,1,0,1,0,1,0,1,0,1,0,1,0,1},
{1,0,0,1,1,1,0,1,0,1,0,1,1,0,1,0,1,1,0,1,1,1,0,1,0,1,0,0,0,0,1,1,0,1,1,0,0,0,1,0,1},
{1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,1,1,0,1,1}};

	//robots maze
	char map[21][41] = {
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, 
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};

	//orientation
	int x = 0;
	int y = 0;
	int dir = iRIGHT; //the robot thinks it's oriented like this
	//start pos
	int startx = 15;
	int starty = 7;
	int startdir = iRIGHT; //implement rotation offset later

	//left first with flips
	/**int flip = 50;
	while(true) {
		flip--;
		if(sonar(x,y,left(dir),(char*)maze,sizex,sizey,startx,starty)) {
			//if 1 then there's a wall to our left
			putwall(x,y,left(dir),(char*)map,sizex,sizey,startx,starty);
			dir = right(dir); //rotate 90 degrees right
			//drawmap((char*)maze,sizex,sizey);
			drawmap((char*)map,sizex,sizey,(x+startx)%sizex,(y+starty)%sizey);
		} else {
			//go left
			dir =  left(dir);
			x = (x + xdelta(dir) + sizex)%sizex;
			y = (y + ydelta(dir) + sizey)%sizey;
		}
		if(!flip) {
			dir = back(dir);
			flip = 50;
		}
	}*/
	
	//unexplored first
	int flip = 100;
	int backtrack = 4;
	while(true) {
		if(sonar(x,y,left(dir),(char*)maze,sizex,sizey,startx,starty)) {
			//if 1 then there's a wall to our left
			putwall(x,y,left(dir),(char*)map,sizex,sizey,0,0);
			dir = right(dir); //rotate 90 degrees right
			//drawmap((char*)maze,sizex,sizey);
			if(!sonar(x,y,left(dir),(char*)map,sizex,sizey,0,0)) drawmap((char*)map,sizex,sizey,(x/*+startx*/)%sizex,(y/*+starty*/)%sizey);
			backtrack = (backtrack==0)?0:(backtrack-1);
		} else {
			if(sonar(x,y,left(dir),(char*)map,sizex,sizey,0,0) && backtrack) {
				//we've already been there
				dir = right(dir); //rotate 90 degrees right
				backtrack--;
			} else {
				mark(x,y,left(dir),(char*)map,sizex,sizey,0,0);
				//go left
				dir =  left(dir);
				x = (x + xdelta(dir) + sizex)%sizex;
				y = (y + ydelta(dir) + sizey)%sizey;
				backtrack = 4;
				flip--;
			}
		}
		if(!flip) {
			dir = back(dir);
			flip = 50;
		}
	}
	
	return 0;
}

//toroid with offset and rotation, ordered as follows:
//pro: -almost no space wasted, no duplicate information (extreme mode: bitfields)
//	   -more space can be saved by leaving out the edges
//     -easy editing, similar to visual representation
//     -not limited to walls, use more bits per element
//con: dimensions (need square for rotation, dims need to be known beforehand)
//size: rows + 1, cols * 2 + 1
//bottom: row + 1, col * 2 + 1
//top: row ==, col * 2
//left: row + 1, col * 2
//right: row + 1, col * 2 + 2
/**

 _ _ _ _ _ _ _ _ _      [[0101010101010101010]
|   |_   _|  _|  _|      [1000110001100110011]      [[00011000110011001]
| | |  _|  _  |_  |  =>  [1010100110010011001]   =>  [01010011001001100]
|_|_ _ _ _ _|_ _ _|      [1111010101011101011]]      [11101010101110101]]

offset example (loop around edges):

_ _ _||_|_ _ _ _ _|     [[1010111111010101011]
_ _ _  _ _ _ _ _ _       [1100111000110001100]      [[10101111010101011]
_|  _||   |_   _|    =>  [0110011010100110010]   =>  [01100010100110010]
 |_  || | |  _|  _       [1010100101010101010]]      [11001000110001100]]

**/

/**
example mazes
20x20
[21][41]
 _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
|_ _ _    | |  _ _ _ _ _  |  _ _ _  |   |
|  _ _ _|_ _  |_ _ _ _   _| |_  |   | | |
| | |_   _ _   _   _  |_ _ _ _ _| |_| | |
| |_ _ _ _  |  _| | |_ _ _ _ _  |_ _ _| |
| |  _ _ _ _|_ _| |  _ _ _ _ _|_ _ _ _ _|
| |   |  _ _ _ _ _ _  |  _ _ _ _  |_ _  |
| | | | |  _ _ _ _  | | |  _ _  |  _ _ _|
| | |_| | |_ _ _  | | | | | |   | |     |
| |   | |_ _ _ _ _| | | | | | |_| | | | |
| | | |  _ _ _ _ _ _| | | | |_ _  | | | |
|_ _|_ _|  _ _ _ _ _ _| |  _|_  | | | | |
| |  _  |  _ _ _ _   _| |_ _   _| | | | |
|_ _|  _|  _ _ _ _ _|_   _  | | | |_ _| |
|  _   _ _ _ _ _ _ _ _ _ _| | | |_ _   _|
|   | |_ _  |  _ _   _ _ _ _| |  _ _ _| |
|_| |_ _  | | |  _  |_ _   _| | |_ _ _  |
|  _|   | | | |_ _| |_ _ _  | |  _  | | |
|_  | |_|_    | | | |  _ _ _| | | | | | |
|  _|_ _ _ _| | |_ _|_ _ _    |_ _|   | |
|_ _ _ _ _ _ _|_ _ _ _ _ _ _|_ _ _ _|_ _|

{
{0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0}, 
{1,1,0,1,0,1,0,0,0,0,1,0,1,0,0,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,0,1,0,1,0,0,1,0,0,0,1},
{1,0,0,1,0,1,0,1,1,1,0,1,0,0,1,1,0,1,0,1,0,1,0,0,0,1,1,0,1,1,0,0,1,0,0,0,1,0,1,0,1},
{1,0,1,0,1,1,0,0,0,1,0,1,0,0,0,1,0,0,0,1,0,0,1,1,0,1,0,1,0,1,0,1,1,0,1,1,1,0,1,0,1},
{1,0,1,1,0,1,0,1,0,1,0,0,1,0,0,1,1,0,1,0,1,1,0,1,0,1,0,1,0,1,0,0,1,1,0,1,0,1,1,0,1},
{1,0,1,0,0,1,0,1,0,1,0,1,1,1,0,1,1,0,1,0,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1,1},
{1,0,1,0,0,0,1,0,0,1,0,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,0,1,0,1,0,1,0,0,1,1,0,1,0,0,1},
{1,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,0,1,0,1,0,0,1,0,0,1,0,1,0,1,1},
{1,0,1,0,1,1,1,0,1,0,1,1,0,1,0,1,0,0,1,0,1,0,1,0,1,0,1,0,1,0,0,0,1,0,1,0,0,0,0,0,1},
{1,0,1,0,0,0,1,0,1,1,0,1,0,1,0,1,0,1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1},
{1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,1,0,1,0,1,1,0,1,0,0,1,0,1,0,1,0,1},
{1,1,0,1,1,1,0,1,1,0,0,1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,0,1,1,1,0,0,1,0,1,0,1,0,1,0,1},
{1,0,1,0,0,1,0,0,1,0,0,1,0,1,0,1,0,1,0,0,0,1,1,0,1,1,0,1,0,0,0,1,1,0,1,0,1,0,1,0,1},
{1,1,0,1,1,0,0,1,1,0,0,1,0,1,0,1,0,1,0,1,1,1,0,0,0,1,0,0,1,0,1,0,1,0,1,1,0,1,1,0,1},
{1,0,0,1,0,0,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,1,0,1,0,1,0,1,1,0,1,0,0,0,1,1},
{1,0,0,0,1,0,1,1,0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,1,0,1,0,1,1,0,1,0,0,1,0,1,0,1,1,0,1},
{1,1,1,0,1,1,0,1,0,0,1,0,1,0,1,0,0,1,0,0,1,1,0,1,0,0,0,1,1,0,1,0,1,1,0,1,0,1,0,0,1},
{1,0,0,1,1,0,0,0,1,0,1,0,1,0,1,1,0,1,1,0,1,1,0,1,0,1,0,0,1,0,1,0,0,1,0,0,1,0,1,0,1},
{1,1,0,0,1,0,1,1,1,1,0,0,0,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,1,0,1,0,1,0,1,0,1,0,1,0,1},
{1,0,0,1,1,1,0,1,0,1,0,1,1,0,1,0,1,1,0,1,1,1,0,1,0,1,0,0,0,0,1,1,0,1,1,0,0,0,1,0,1},
{1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,0,1,0,1,0,1,1,1,0,1,0,1,0,1,1,1,0,1,1}};

*/